
// 2024/2/26: 首个版本
// 2024/4/12: 补上ToGeneralSample参数

using System;
using System.Linq;
using ASEva;
using SO = System.Reflection.ObfuscationAttribute;

namespace VehicleControlV1
{
    [SO]
	enum LongitudinalControlMode
	{
		NoRequest = 0, // Not requesting longitudinal control / 不请求纵向控制
		Position = 1, // Control throttle and brake pedal, value ranges from -1(max brake) ~ 1(max throttle) / 控制油门、刹车踏板，数值范围为-1(刹车到底)~1(油门到底)
		Acceleration = 2, // Control acceleration and deceleration, in m/s² units (The target absolute speed is necessary to set) / 控制加、减速度，数值单位为m/s²，同时需要指定目标绝对速度
	}

    [SO]
	enum LateralControlMode
	{
		NoRequest = 0, // Not requesting lateral control / 不请求横向控制
		Position = 1, // Control steer, value ranges from -1(max right turning) ~ 1(max left turning) / 控制方向盘，数值范围为-1(右打到底)~1(左打到底)
		Curvature = 2, // Control turning curvature, in 1/m units / 控制转弯曲率，数值单位为1/m
	}

    [SO]
	enum GearControlMode
	{
		NoRequest = 0, // Not requesting gear switching / 不请求换挡
		Drive = 1, // Switch to drive gear / 请求换D挡
		Reverse = 2, // Switch to reverse gear / 请求换R挡
	}

    class VehicleControl : Sample
    {
		public LongitudinalControlMode LongitudinalMode { get; set; } // 纵向控制模式
		public Double LongitudinalValue { get; set; } // 纵向控制数值
		public Double TargetSpeed { get; set; } // [KPH] Target absolute speed, only available on acceleration control mode / 目标绝对速度，仅限加速度控制模式下有效

		public LateralControlMode LateralMode { get; set; } // 横向控制模式
		public Double LateralValue { get; set; } // 横向控制数值

		public GearControlMode GearMode { get; set; } // 换挡控制模式

        public VehicleControl()
        {
			LongitudinalMode = LongitudinalControlMode.NoRequest;
			LongitudinalValue = 0;
			TargetSpeed = 0;

			LateralMode = LateralControlMode.NoRequest;
			LateralValue = 0;

			GearMode = GearControlMode.NoRequest;
        }

        public static string Title
        {
            get
            {
                return "Longitudinal control mode,Longitudinal target value,Target speed[KPH],Lateral control mode,Lateral target value,Gear control mode";
            }
        }

        public static string Protocol
        {
            get
            {
                return "vehicle-control-v1";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new String[] { "vehicle-control-v1" }; 
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample(int channel)
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;
            sample.Channel = channel;

            var data = new GeneralSampleValue[6];

            data[0] = new GeneralSampleValue((double)(int)LongitudinalMode);
            data[1] = new GeneralSampleValue(LongitudinalValue);
            data[2] = new GeneralSampleValue(TargetSpeed);
            data[3] = new GeneralSampleValue((double)(int)LateralMode);
            data[4] = new GeneralSampleValue(LateralValue);
            data[5] = new GeneralSampleValue((double)(int)GearMode);

            sample.Values = data.ToList();
            sample.NumberOfSignificants = data.Length;

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);

            if (sample.Protocol == "vehicle-control-v1")
            {
                if (sample.Values.Count != 6) return false;

                LongitudinalMode = sample.Values[0].IsNumber() ? (LongitudinalControlMode)(int)sample.Values[0].number : LongitudinalControlMode.NoRequest;
                LongitudinalValue = sample.Values[1].IsNumber() ? sample.Values[1].number : 0.0;
                TargetSpeed = sample.Values[2].IsNumber() ? sample.Values[2].number : 0.0;
                LateralMode = sample.Values[3].IsNumber() ? (LateralControlMode)(int)sample.Values[3].number : LateralControlMode.NoRequest;
                LateralValue = sample.Values[4].IsNumber() ? sample.Values[4].number : 0.0;
                GearMode = sample.Values[5].IsNumber() ? (GearControlMode)(int)sample.Values[5].number : GearControlMode.NoRequest;

                return true;
            }

            return false;
        }
    }
}